#include "AnimCreature.h"

#include <algorithm>


#include "OgreFrameWork.h"

#include "MovableObject.h"

float AnimCreature::getDistanceFromDestination()
{
    return m_position.distance(m_destination);
}


AnimCreature::AnimCreature(MovableObject* _parent):
m_parent(_parent)
{
    //ctor
    m_heading = 0;
    m_position = Ogre::Vector3(8,0,8);
    m_currentPosition = m_position;

    m_turnRate=0.01;
    m_velocity=0.001;

    m_velocityAdjustWhenTurning=0.3;

    m_wobbleVelocity = Ogre::Vector3(0,0,0);

    float legLength = 32;
    float baseWidth = 8;

    m_legs.push_back(Leg(this,Ogre::Vector2(baseWidth,8), Ogre::Vector2(20,20)));
    m_legs.push_back(Leg(this,Ogre::Vector2(-baseWidth,8), Ogre::Vector2(20,20)));
    m_legs.push_back(Leg(this,Ogre::Vector2(baseWidth,8), Ogre::Vector2(4,20)));
    m_legs.push_back(Leg(this,Ogre::Vector2(-baseWidth,8), Ogre::Vector2(4,20)));

}

/** @brief init
  *
  * @todo: document this function
  */
void AnimCreature::init()
{
m_position = addYtoVector2(m_parent->getPosition(),0.5);
m_currentPosition=m_position;
m_destination=m_position;
}






/** @brief update
  *
  * @todo: document this function
  */
void AnimCreature::update(double timeSinceLastFrame)
{

    float targetHeading ;

    m_position =addYtoVector2(m_parent->getPosition(),0.5);
    Ogre::Vector2 dir = Ogre::Vector2 (m_destination.x-m_position.x,m_destination.z-m_position.z);
    dir.normalise();

    targetHeading = atan2(dir.y,dir.x);
    float  dHeading = modulo(targetHeading-m_heading + 8 * TWO_PI,TWO_PI);

    if (dHeading > PI)
    {
        dHeading -= TWO_PI;
    }
    if (dHeading < -m_turnRate)
    {
        m_heading -= m_turnRate * timeSinceLastFrame;
    }
    else if (dHeading > m_turnRate)
    {
        m_heading += m_turnRate * timeSinceLastFrame;
    }




    m_velocityAdjustWhenTurning = std::max((double)0.2f, (double) -3.0+4.0*(1-abs(dHeading)/PI));
    m_heading = modulo ((m_heading + TWO_PI) , TWO_PI);



    float dX = cos(m_heading);
    float dY = sin(m_heading);
    m_position.x += dX * m_velocityAdjustWhenTurning * m_velocity * timeSinceLastFrame;
    m_position.z += dY * m_velocityAdjustWhenTurning * m_velocity * timeSinceLastFrame;

m_currentPosition = m_position;

m_parent->setPosition(removeYtoVector3(m_position));
/*

          Ogre::Vector3 acceleration = m_position-m_currentPosition;

        acceleration.normalise();
        acceleration = 0.0003*acceleration;

        m_wobbleVelocity = m_wobbleVelocity + acceleration;
        m_wobbleVelocity = 0.99*m_wobbleVelocity;
        m_currentPosition+= m_wobbleVelocity;
*/
}



float AnimCreature::getAngleHeading()
{
    return m_heading;
}


AnimCreature::~AnimCreature()
{
    //dtor
}


/** @brief update
  *
  * @todo: document this function
  */
void Leg::update(int _turnDirection)
{

}

/** @brief ~Leg
  *
  * @todo: document this function
  */
Leg::~Leg()
{

}

/** @brief Leg
  *
  * @todo: document this function
  */
Leg::Leg(AnimCreature* _parent, Ogre::Vector2 _start, Ogre::Vector2 _dif):
    m_parent(_parent)
{
    m_foot = Ogre::Vector2(m_parent->getPosition2D()+_dif);
    m_footCenterRelative = _dif;
    m_footCenter = Ogre::Vector2(0,0);
    m_attachPositionRelative = _start;
    m_goalPosition = m_footCenter;
    m_facing = int(_dif.x / abs(_dif.x));
    m_baseAngle = atan2(_dif.y-_start.y, _dif.x - _start.x) - PI / 2;
    update(0);


}


